#include "StdAfx.h"
#include "Ray.h"
#include "CoordFrame.h"
#include "Plane.h"



CRay::CRay(void): m_position(m_center)
{
	alignRayVectorOnY();
	//installSupportObject(new CCoordFrame());
}
CRay::CRay(const CPoint3D& position, const CVector3D& dirVector): m_position(m_center)
{
	m_position			= position;
	m_dirVector			= dirVector;
	m_dirVector.normalize();

	alignRayVectorOnY();
	//installSupportObject(new CCoordFrame());
}
CRay::CRay(const CPoint3D& from, const CPoint3D& to): m_position(m_center)
{
	m_position		= from;
	m_dirVector		= to - from;
	m_dirVector.normalize();

	alignRayVectorOnY();
	//installSupportObject(new CCoordFrame());
}

CRay::~CRay(void)
{
}
void CRay::setPosition(const CPoint3D position){
	m_position	= position;
}
const CPoint3D& CRay::getPosition() const{
	return m_position;
}
void CRay::setDirection(const CVector3D dirVector){
	m_dirVector = dirVector;
	m_dirVector.normalize();
	m_rotObject.loadIdentity();

	alignRayVectorOnY();
}
const CVector3D& CRay::getDirection() const{
	return m_dirVector;
}
CRay& CRay::operator=(const CRay& ray){
	this->m_position = ray.getPosition();
	this->m_dirVector = ray.getDirection();
	return *this;
}
void CRay::alignRayVectorOnY(){
	//float nx = m_dirVector.getX();
	//float ny = m_dirVector.getY();
	//float nz = m_dirVector.getZ();
	//if ((m_dirVector.getNorm() == 0) || ((nx == 0) && (nz == 0) )) return;

	//float thetaY, thetaX;
	//float d;
	//thetaY			= atan2(nx,nz);

	//nz = abs(nz); //nz in the new reference frame, i.e. after rotating around Y-axis a thetaY
	//
	//d = sqrt(nx*nx + nz*nz);
	//if(ny >= 0){
	//	thetaX = asin(d);
	//}
	//else{
	//	thetaX = PI - asin(d);
	//}
	////Note:		thetaY : [-PI, PI]
	////			thetaX : [0, PI]
	//
	//thetaY			= thetaY*RAD_2_DEG;
	//thetaX			= thetaX*RAD_2_DEG;

	////Find vectors Y and X in the internal master frame 
	//CVector3D vecY_InInternalMaster, vecX_InInternalMaster;
	//if(m_pExternalMaster != NULL){
	//	vecY_InInternalMaster = (m_rotExternalMaster*m_rotE_EI*m_rotEI_I)*CVector3D(0,1,0);
	//	vecX_InInternalMaster = (m_rotExternalMaster*m_rotE_EI*m_rotEI_I)*CVector3D(1,0,0);
	//}
	//else{
	//	vecY_InInternalMaster = (m_rotGlobal*m_rotInternalMaster)*CVector3D(0,1,0);
	//	vecX_InInternalMaster = (m_rotGlobal*m_rotInternalMaster)*CVector3D(1,0,0);
	//}
	//
	//CQuaternion alignRot	= CQuaternion(vecY_InInternalMaster, thetaY)*CQuaternion(vecX_InInternalMaster, thetaX);
	//m_rotObject				= alignRot * m_rotObject;
}

void CRay::Draw(){
	//glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, getAmbiFactor(0));
	//glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, getDiffFactor(0));
	//glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, getSpecFactor(0));
	//glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, getShinFactor(0));

	//glLineWidth(m_lineWidth);

	glBegin(GL_LINES);
		glNormal3fv(m_dirVector);
		glVertex3f(0, 0, 0);
		glNormal3fv(m_dirVector);
		glVertex3f(0, 1, 0);
	glEnd();
}

int CRay::intersects(const CPlane& plane, CPoint3D& intersectPoint){
	CVector3D	u = m_dirVector;
	CVector3D	w = m_position - plane.getPosition();
	CVector3D	planeNormal = plane.getNormal();
	float		D = planeNormal.dotProduct(u);
	float		N = - planeNormal.dotProduct(w);

	if(abs(D) < SMALL_NUM){
		if(N == 0)
			return 2;
		else
			return 0;
	}
	float s		= N / D;
	intersectPoint = m_position + s*u;         // compute segment intersect point
	return 1;
}
